#ifndef INNFOS_KINEMATICS_H
#define INNFOS_KINEMATICS_H


#include <Eigen/Eigen>
#include <vector>

class innfos_kinematics {

public:
    using Vector6 = Eigen::Matrix<double, 1, 6>;
    using Tranform44 = Eigen::Isometry3d;

    innfos_kinematics();

    Eigen::Isometry3d baseTend(Vector6 q);
    int solve(const Eigen::Isometry3d& transform3D, std::vector<Vector6>& joints, double eps=1e-10);
    int getQ(const Eigen::Isometry3d& transform3D, Vector6 q, Vector6 &targetQ);
    double norm2(Vector6 q1, Vector6 q2);

private:
    Eigen::Matrix<double,7,1> _a;
    Eigen::Matrix<double,7,1> _alpha;
    Eigen::Matrix<double,7,1> _d;
    Eigen::Matrix<double,7,1> _theta;

    Eigen::Matrix<double, 3, 3> r;
    Eigen::Matrix<double, 3, 1> p;

    double d2;
    double a3;
    double a4;
    double d5;
    double d6;
    double d7;

    double _eps;

    Eigen::Isometry3d Transform3D_DH(double a,double alpha,double d, double theta);

    double solveQ0(bool positive);
    double solveQ4(double q0,bool positive);
    double solveQ5(double q0,double q4);
    double solveQ123(double q0,double q4);
    double solveQ2(double q0,double q4,double q123,bool positive);
    double solveQ1(double q0,double q4,double q123,double q2);
    double solveQ3(double q123,double q2,double q1);
    double M(double q0, double q4,double q123);
    double N(double q0, double q4,double q123);
};

inline double innfos_kinematics::norm2(Vector6 q1, Vector6 q2)
{
    Vector6 q = q2 - q1;

    return sqrt(q * q.transpose());
}

#endif // INNFOS_KINEMATICS_H
